/*
 * NewtonSolver.cpp
 *
 *  Created on: 20 Oct 2011
 *      Author: allan
 */

#include "NewtonSolver.h"

// GSL includes
#include <gsl/gsl_linalg.h>

// GeoReact includes
#include <Utils/Assert.h>

NewtonSolver::NewtonSolver() :
decomposition(FullPivotingLU), tolerance(1.0E-6), maxiterations(100)
{}

NewtonSolver::NewtonSolver(unsigned dimension) :
decomposition(FullPivotingLU), tolerance(1.0E-6), maxiterations(100)
{
	Initialiaze(dimension);
}

void NewtonSolver::Initialiaze(unsigned dimension)
{
	F = VectorXd::Zero(dimension);
	J = MatrixXd::Zero(dimension, dimension);
}

void NewtonSolver::SetDecompositionAlgorithm(Decomposition decomposition)
{
	this->decomposition = decomposition;
}

void NewtonSolver::SetConvergenceOptions(double tolerance, uint maxiterations)
{
	this->tolerance = tolerance;
	this->maxiterations = maxiterations;
}

const VectorXd& NewtonSolver::GetResidualFunction() const
{
	return F;
}

const VectorXd& NewtonSolver::GetDelta() const
{
	return dx;
}

/**/
bool NewtonSolver::Iterate(NewtonProblem& problem, VectorXd& x)
{
	problem.Function(x, F);
	problem.Jacobian(x, J);
	
	dx = J.lu().solve(-F);
	
	const double relative_error = (J*dx + F).norm() / F.norm();
	
	Assert(relative_error < 1.0E-8, "The linear system could not be solved.");
	
	x += dx;
	
	return (F.norm() < tolerance);
}
//*/

// The use of Eigen for the LU decomposition is slight faster than GSL (about 10-15%)
/*
bool NewtonSolver::Iterate(NewtonProblem& problem, VectorXd& x)
{
	problem.Function(x, F);
	problem.Jacobian(x, J);
	
	F *= -1.0;
	
	//gsl_matrix_view gsl_J = gsl_matrix_view_array(J.data(), J.rows(), J.cols());
	
	gsl_matrix* gsl_J = gsl_matrix_alloc(J.rows(), J.cols());
	
	for(unsigned i = 0; i < J.rows(); ++i) for(unsigned j = 0; j < J.cols(); ++j)
		gsl_matrix_set(gsl_J, i, j, J(i,j));
		
	gsl_vector_view gsl_minus_F = gsl_vector_view_array(F.data(), F.rows());
	
	gsl_vector* gsl_dx = gsl_vector_alloc(x.rows());
	
	gsl_permutation* p = gsl_permutation_alloc(x.rows());
	
	int signum;
	
	gsl_linalg_LU_decomp(gsl_J, p, &signum);
	
	gsl_linalg_LU_solve(gsl_J, p, &gsl_minus_F.vector, gsl_dx);
	
	VectorXd dx(x.rows()); for(unsigned i = 0; i < x.rows(); ++i) dx[i] = gsl_vector_get(gsl_dx, i);
	
	gsl_permutation_free(p);
	gsl_matrix_free(gsl_J);
	gsl_vector_free(gsl_dx);
	
	x += dx;
	
	return (F.norm() < tolerance);
}
//*/

ConvergenceDiagnostics NewtonSolver::Solve(NewtonProblem& problem, VectorXd& x)
{
	ConvergenceDiagnostics diagnostics = {0, false};
	
	while(!diagnostics.converged && diagnostics.num_iterations++ < maxiterations) 
		diagnostics.converged = Iterate(problem, x);
	
	return diagnostics;
}

